#include "taiic_rkisp.h"
#include "taiic_rkisp_config.h"

void errno_exit(taiic_rkisp_s *ctx, const char *s)
{
    ERR("%s: %s error %d, %s\n", get_sensor_name(ctx), s, errno, strerror(errno));
}
char* get_sensor_name(taiic_rkisp_s* ctx)
{
    return ctx->sns_name;
}
char* get_dev_name(taiic_rkisp_s* ctx)
{
    return ctx->dev_name;
}

int xioctl(int fh, int request, void *arg)
{
    int r;
    do {
        r = ioctl(fh, request, arg);
    } while (-1 == r && EINTR == errno);
    return r;
}


void open_device(taiic_rkisp_s *ctx)
{
    RK_LOGD("-------- open output dev_name:%s -------------\n", get_dev_name(ctx));
    ctx->fd = open(get_dev_name(ctx), O_RDWR /* required */ /*| O_NONBLOCK*/, 0);

    if (-1 == ctx->fd) {
        ERR("Cannot open '%s': %d, %s\n",
            get_dev_name(ctx), errno, strerror(errno));
        exit(EXIT_FAILURE);
    }
}


void init_device(taiic_rkisp_s *ctx)
{
    struct v4l2_capability cap;
    struct v4l2_format fmt;

    if (-1 == xioctl(ctx->fd, VIDIOC_QUERYCAP, &cap)) {
        if (EINVAL == errno) {
            ERR("%s: %s is no V4L2 device\n", get_sensor_name(ctx),
                get_dev_name(ctx));
        } else {
            errno_exit(ctx, "VIDIOC_QUERYCAP");
        }
    }

    RK_LOGD("==This is line %d in file==\n", __LINE__);
    ctx->buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    CLEAR(fmt);
    fmt.type = ctx->buf_type;
    fmt.fmt.pix_mp.width = ctx->width;
    fmt.fmt.pix_mp.height = ctx->height;
    fmt.fmt.pix_mp.pixelformat = ctx->format;
    fmt.fmt.pix_mp.field = V4L2_FIELD_INTERLACED;

    fmt.fmt.pix_mp.quantization = V4L2_QUANTIZATION_FULL_RANGE;
    RK_LOGD("==This is line %d in file==\n", __LINE__);

    if (-1 == xioctl(ctx->fd, VIDIOC_S_FMT, &fmt))
        errno_exit(ctx, "VIDIOC_S_FMT");

    init_mmap(false, ctx);
}

void init_mmap(int pp_onframe, taiic_rkisp_s *ctx)
{
    struct v4l2_requestbuffers req;
    int fd_tmp = -1;

    CLEAR(req);

    fd_tmp = ctx->fd;

    req.count = BUFFER_COUNT;
    req.type = ctx->buf_type;
    req.memory = V4L2_MEMORY_MMAP;

    struct buffer *tmp_buffers = NULL;

    if (-1 == xioctl(fd_tmp, VIDIOC_REQBUFS, &req)) {
        if (EINVAL == errno) {
            ERR("%s: %s does not support "
                "memory mapping\n", get_sensor_name(ctx), get_dev_name(ctx));
        } else {
            errno_exit(ctx, "VIDIOC_REQBUFS");
        }
    }
        tmp_buffers = (struct buffer*)calloc(req.count, sizeof(struct buffer));

    if (!tmp_buffers) {
        ERR("%s: Out of memory\n", get_sensor_name(ctx));
    }

    ctx->buffers = tmp_buffers;

    for (ctx->n_buffers = 0; ctx->n_buffers < req.count; ++ctx->n_buffers) {
        struct v4l2_buffer buf;
        struct v4l2_plane planes[FMT_NUM_PLANES];
        CLEAR(buf);
        CLEAR(planes);

        buf.type = ctx->buf_type;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = ctx->n_buffers;


        buf.m.planes = planes;
        buf.length = FMT_NUM_PLANES;


        if (-1 == xioctl(fd_tmp, VIDIOC_QUERYBUF, &buf))
            errno_exit(ctx, "VIDIOC_QUERYBUF");


        tmp_buffers[ctx->n_buffers].length = buf.m.planes[0].length;
        tmp_buffers[ctx->n_buffers].start =
                mmap(NULL /* start anywhere */,
                     buf.m.planes[0].length,
                     PROT_READ | PROT_WRITE /* required */,
                     MAP_SHARED /* recommended */,
                     fd_tmp, buf.m.planes[0].m.mem_offset);

        if (MAP_FAILED == tmp_buffers[ctx->n_buffers].start)
            errno_exit(ctx, "mmap");

  
        struct v4l2_exportbuffer expbuf;
        xcam_mem_clear (expbuf);
        expbuf.type = ctx->buf_type;
        expbuf.index = ctx->n_buffers;
        expbuf.flags = O_CLOEXEC;
        if (xioctl(fd_tmp, VIDIOC_EXPBUF, &expbuf) < 0) {
            errno_exit(ctx, "get dma buf failed\n");
        } else {
            DBG("%s: get dma buf(%d)-fd: %d\n", get_sensor_name(ctx), ctx->n_buffers, expbuf.fd);
        }
        tmp_buffers[ctx->n_buffers].export_fd = expbuf.fd;
    }
}
void start_capturing(taiic_rkisp_s *ctx)
{
    unsigned int i;
    enum v4l2_buf_type type;

    for (i = 0; i < ctx->n_buffers; ++i) {
        struct v4l2_buffer buf;

        CLEAR(buf);
        buf.type = ctx->buf_type;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;

        if (V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE == ctx->buf_type) {
            struct v4l2_plane planes[FMT_NUM_PLANES];

            buf.m.planes = planes;
            buf.length = FMT_NUM_PLANES;
        }
        if (-1 == xioctl(ctx->fd, VIDIOC_QBUF, &buf))
            errno_exit(ctx, "VIDIOC_QBUF");
    }
    type = ctx->buf_type;
    DBG("%s:-------- stream on output -------------\n", get_sensor_name(ctx));

    if (-1 == xioctl(ctx->fd, VIDIOC_STREAMON, &type))
        errno_exit(ctx, "VIDIOC_STREAMON");
}

int read_frame(taiic_rkisp_s *ctx)
{
    struct v4l2_buffer buf;
    int i, bytesused;

    CLEAR(buf);

    buf.type = ctx->buf_type;
    buf.memory = V4L2_MEMORY_MMAP;

    struct v4l2_plane planes[FMT_NUM_PLANES];
    memset(planes, 0, sizeof(struct v4l2_plane)*FMT_NUM_PLANES);

    buf.m.planes = planes;
    buf.length = FMT_NUM_PLANES;

    if (-1 == xioctl(ctx->fd, VIDIOC_DQBUF, &buf))
        errno_exit(ctx, "VIDIOC_DQBUF");

    i = buf.index;
    bytesused = buf.m.planes[0].bytesused;

    process_image(ctx->buffers[i].start,  buf.sequence, bytesused, ctx);
    if (-1 == xioctl(ctx->fd, VIDIOC_QBUF, &buf))
            errno_exit(ctx, "VIDIOC_QBUF");

    return 1;
}

void process_image(const void *p, int sequence, int size, taiic_rkisp_s *ctx)
{
    if (ctx->fp && sequence >= ctx->skipCnt && ctx->outputCnt-- > 0) {
        RK_LOGD(">\n");
        fwrite(p, size, 1, ctx->fp);
        fflush(ctx->fp);
        fsync(fileno(ctx->fp));
        
    } else if (ctx->fp && sequence >= ctx->skipCnt && ctx->outputCnt-- < 0) {
        fclose(ctx->fp);
        ctx->fp = NULL;
        RK_LOGD("==This is line %d in file %s==\n", __LINE__, __FILE__);

    } 
}

void stop_capturing(taiic_rkisp_s *ctx)
{
    enum v4l2_buf_type type;

    type = ctx->buf_type;
    if (-1 == xioctl(ctx->fd, VIDIOC_STREAMOFF, &type))
        errno_exit(ctx, "VIDIOC_STREAMOFF");
}


void close_device(taiic_rkisp_s *ctx)
{
    if (-1 == close(ctx->fd))
        errno_exit(ctx, "close");

    ctx->fd = -1;
}

void uninit_device(taiic_rkisp_s *ctx)
{
    unsigned int i;
    if (ctx->n_buffers == 0)
        return;

    for (i = 0; i < ctx->n_buffers; ++i) {
        if (-1 == munmap(ctx->buffers[i].start, ctx->buffers[i].length))
            errno_exit(ctx, "munmap");

        close(ctx->buffers[i].export_fd);
    }

    free(ctx->buffers);
    ctx->n_buffers = 0;
}

void deinit_rkisp(taiic_rkisp_s *ctx)
{
    

    printf("%s:-------- stop aiq -------------\n", get_sensor_name(ctx));
    rk_aiq_uapi2_sysctl_stop(ctx->aiq_ctx, false);
    RK_LOGD("%s:-------- deinit aiq -------------\n", get_sensor_name(ctx));

    rk_aiq_uapi2_sysctl_deinit(ctx->aiq_ctx);
    RK_LOGD("%s:-------- deinit aiq end -------------\n", get_sensor_name(ctx));

    
    if (ctx->fp)
        fclose(ctx->fp);
}



void init_rkisp_routine(taiic_rkisp_s *ctx)
{
    char sns_entity_name[64];
    rk_aiq_working_mode_t work_mode = RK_AIQ_WORKING_MODE_NORMAL;

    RK_LOGD("work_mode %d\n", work_mode);

    strcpy(sns_entity_name, rk_aiq_uapi2_sysctl_getBindedSnsEntNmByVd(get_dev_name(ctx)));
    RK_LOGD("sns_entity_name:%s\n", sns_entity_name);
    sscanf(&sns_entity_name[6], "%s", ctx->sns_name);
    RK_LOGD("sns_name:%s\n", ctx->sns_name);

    rk_aiq_static_info_t s_info;
    rk_aiq_uapi2_sysctl_getStaticMetas(sns_entity_name, &s_info);

    RK_LOGD("%s:-------- open output dev -------------\n", get_sensor_name(ctx));
    open_device(ctx);

    if (ctx->writeFile) {
        ctx->fp = fopen(ctx->out_file, "w+");
        RK_LOGD("==This is line %d in file==\n", __LINE__);
        if (ctx->fp == NULL) {
            ERR("%s: fopen output file %s failed!\n", get_sensor_name(ctx), ctx->out_file);
        }
    }

    XCamReturn ret = XCAM_RETURN_NO_ERROR;
    rk_aiq_tb_info_t tb_info;
    tb_info.magic = sizeof(rk_aiq_tb_info_t) - 2;
    tb_info.is_pre_aiq = false;
    RK_LOGD("==This is line %d in file==\n", __LINE__);
    ret = rk_aiq_uapi2_sysctl_preInit_tb_info(sns_entity_name, &tb_info);

    RK_LOGD("==This is line %d in file==\n", __LINE__);
    ret = rk_aiq_uapi2_sysctl_preInit_scene(sns_entity_name, "normal", "day");

    if (ret < 0)
        ERR("%s: failed to set %s scene\n",
    get_sensor_name(ctx),
    work_mode == RK_AIQ_WORKING_MODE_NORMAL ? "normal" : "hdr");


    RK_LOGD("==This is line %d in file==\n", __LINE__);
    ctx->aiq_ctx = rk_aiq_uapi2_sysctl_init(sns_entity_name, ctx->iqpath, NULL, NULL);
        
    RK_LOGD("%s:-------- init mipi tx/rx -------------\n", get_sensor_name(ctx));

    ret = rk_aiq_uapi2_sysctl_prepare(ctx->aiq_ctx, ctx->width, ctx->height, work_mode);
    RK_LOGD("===This is line %d in the file===\n",__LINE__);
    if (ret != XCAM_RETURN_NO_ERROR)
        ERR("%s:rk_aiq_uapi2_sysctl_prepare failed: %d\n", get_sensor_name(ctx), ret);
    else {
        RK_LOGD("===This is line %d in the file===\n",__LINE__);
        ret = rk_aiq_uapi2_setMirrorFlip(ctx->aiq_ctx, false, false, 3);
     
        ret = rk_aiq_uapi2_sysctl_start(ctx->aiq_ctx );

        RK_LOGD("%s:-------- start success -------------\n", ctx->sns_name);
    }
}


void init_rkisp_ctx(taiic_rkisp_s *ctx)
{
    strcpy(ctx->out_file, OUT_FILE);
    strcpy(ctx->dev_name, DEVICE_NAME);
    // ctx->sns_name = {'\0'};
    ctx->width = IMG_WIDTH;
    ctx->height = IMG_HEIGHT;
    ctx->format = FORMAT;
    ctx->fd =-1;
    ctx->buf_type = BUFFER_TYPE;
    ctx->buffers = NULL;
    ctx->n_buffers = 0;
    ctx->frame_count = -1;
    ctx->fp =NULL;
    ctx->aiq_ctx = NULL;
    ctx->rkaiq = 1;
    ctx->writeFile = 1;
    ctx->outputCnt = IMAGE_COUNT;
    ctx->skipCnt = IMAGE_SKIP;
    ctx->ctl_type = CTL_TYPE;
    strcpy(ctx->iqpath, IQPATH);
}

